# 这里尝试利用gazebo实现基于diablo模型的仿真

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node


def generate_launch_description():

    shared_pkg = get_package_share_directory('imu_tool')

    #######################
    # Declare Arguments
    ######################
    imu_topic = LaunchConfiguration('imu_topic')
    declare_imu_topic = DeclareLaunchArgument(
        name = 'imu_topic',
        default_value="/imu/data",
        description='Absolute path to imu topic')


    ######################
    # Specify the actions
    ######################

    imu2odom_cmd = Node(
        package='imu_tool',
        executable='compute_gravity',
        name='compute_gravity',
        output='screen',
        parameters=[{'imu_topic': imu_topic}],
    )


    return LaunchDescription([
        declare_imu_topic,

        imu2odom_cmd,
    ])
